#ifndef _SLAM_H_
#define _SLAM_H_

#include "posinfo.hpp"
#include <cstring>
#include <iostream>
#include <thread>

#include <unitree/common/dds/dds_easy_model.hpp>
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/ros2_idl/Odometry_.hpp>
#include <unitree/ros2_idl/QtCommand_.hpp>
#include <unitree/ros2_idl/QtEdge_.hpp>
#include <unitree/ros2_idl/QtNode_.hpp>
#include <unitree/ros2_idl/String_.hpp>

#define COMMANDTOPIC "rt/qt_command"
#define NOTICETOPIC "rt/qt_notice"
#define ODOMTOPIC "rt/lio_sam_ros2/mapping/re_location_odometry"
#define ADDNODETOPIC "rt/qt_add_node"
#define ADDEDGETOPIC "rt/qt_add_edge"

using namespace unitree::robot;

class Slam {
private:
    int index = 0;
    bool running = false;
    std::thread handle;
    const nav_msgs::msg::dds_::Odometry_* odom = NULL;

    ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtCommand_> pubQtCommand
        = ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtCommand_>(new ChannelPublisher<unitree_interfaces::msg::dds_::QtCommand_>(COMMANDTOPIC));
    ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtNode_> pubQtNode
        = ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtNode_>(new ChannelPublisher<unitree_interfaces::msg::dds_::QtNode_>(ADDNODETOPIC));
    ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtEdge_> pubQtEdge
        = ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtEdge_>(new ChannelPublisher<unitree_interfaces::msg::dds_::QtEdge_>(ADDEDGETOPIC));

    ChannelSubscriberPtr<std_msgs::msg::dds_::String_> subQtNotice
        = ChannelSubscriberPtr<std_msgs::msg::dds_::String_>(new ChannelSubscriber<std_msgs::msg::dds_::String_>(NOTICETOPIC));
    ChannelSubscriberPtr<nav_msgs::msg::dds_::Odometry_> subOdommetry
        = ChannelSubscriberPtr<nav_msgs::msg::dds_::Odometry_>(new ChannelSubscriber<nav_msgs::msg::dds_::Odometry_>(ODOMTOPIC));

    void relocation();
    void post_init();
    void nav_open();

public:
    Slam();
    ~Slam();

    void qtNoticeHandler(const void* message);
    void odometryHandler(const void* message);
    void nav_start();
    void nav_pause();
    void nav_recover();
    void pos_get(float& x, float& y, float& yaw);
    void pos_thread();
    void pos_thread_start();
    void pos_thread_stop();
    void msleep(int ms);
};

#endif
